package Controller;

import lejos.nxt.LightSensor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;

public class LineFollower implements Behavior{
	private boolean suppressed = false;
	private TachoPilot pilot;
	private LightSensor lightSensor;
	private int toFollow;
	private int offset;	
		
	public LineFollower(LightSensor lightSensor, TachoPilot pilot, int toFollow, int offset){
		this.lightSensor = lightSensor;
		this.pilot = pilot;
		this.offset = offset;
		this.toFollow = toFollow;
	}
	
	@Override
	public boolean takeControl(){		
		return lineFound();
	}	
	
	@Override
	public void action(){
		suppressed = false;
		int angle = 10;
		boolean lineFound = false;
		while(!lineFound){
			pilot.travel(5);
			pilot.rotate(-angle); //beginnen links van de lijn, begin met linksdraai
			if(lineFound()) break;
			angle += 10;			
		}
		lineFound = false; angle = 10;
		while(!lineFound){
			pilot.travel(5);
			pilot.rotate(angle); //beginnen links van de lijn, begin met linksdraai
			if(lineFound()) break;
			angle += 10;			
		}	
	}
	
	@Override
	public void suppress(){
		pilot.stop();
		suppressed = true;
	}
	
	private boolean lineFound(){
		return (lightSensor.readValue() < toFollow + offset) || (lightSensor.readValue() > toFollow - offset);
	}
}
